package endcourseproject;

import java.util.Enumeration;
import java.util.Hashtable;
import lejos.nxt.LCD;
import lejos.robotics.navigation.CompassPilot;
import lejos.robotics.navigation.Pilot;

public class Navigator {
	
	public static boolean avoid = false;
	
	private Hashtable _behaviors = new Hashtable();
	private Thread _thread = null;
	private boolean _started= false;
	private Pilot _pilot;
	private RobotManager _robotMan;
	private final float MAX_MAGNITUDE = 10f;
	private static final int FORWARD = 1;
	private static final int BACKWARD = -1;
	float max_speed;
	
	private boolean _useSteer = false;
	
	public Navigator(Pilot pilot, RobotManager rm, boolean useSteer)
	{
		_useSteer = useSteer;
		_pilot = pilot;
		_robotMan = rm;
		
		synchronized (_pilot) {
			
			max_speed = _pilot.getMoveMaxSpeed();
        	_pilot.setMoveSpeed(max_speed / 3);
        	max_speed = _pilot.getTurnMaxSpeed();
			_pilot.setTurnSpeed(max_speed / 3);
		}
		
		_thread = new Thread()
		{
			// Inline thread for handling navigation
			public void run()
			{
				Object key;
				int priority;
				double x1, y1;
				double magnitude;
				double direction;
				double x = 0;
				double y = 0;
				int behaviourNum = 0;
				int banan = 0;
				
				
				
				
				
				// Forever
				while(true)
				{
					LCD.drawString(1+"   ", 10, 3);
					// Reset variables;
					priority = behaviourNum = 0;
					x1 = y1 = magnitude = direction = x = y = 0;
					
					// Enumerate behaviors
					Enumeration keys = _behaviors.keys();
					while(keys.hasMoreElements())
					{
						key = keys.nextElement();
						if( key != null )
						{
							Behavior b = (Behavior)key;
							
							// Extract direction and magnitude
							direction = b.getDirection();
							magnitude = b.getMagnitude();
							// Min/max magnitude
							if( magnitude > MAX_MAGNITUDE) {magnitude = MAX_MAGNITUDE; }
							if( magnitude < 0) { magnitude = 0; }
							
							// we now know magnitude is between 0 and MAX_MAGNITUDE (10)
							//_pilot.setMoveSpeed((int)(max_speed*magnitude)/10);
							
							if( b instanceof Avoid)
							{
								_robotMan.getRobotStatus().setBehaviourAngle(0,(int)direction);
								_robotMan.getRobotStatus().setBehaviourMagnitude(0,(int)magnitude);	
							}
							else if( b instanceof UltraAvoid)
							{
								_robotMan.getRobotStatus().setBehaviourAngle(1,(int)direction);
								_robotMan.getRobotStatus().setBehaviourMagnitude(1,(int)magnitude);	
							}
							else if( b instanceof Flock)
							{
								_robotMan.getRobotStatus().setBehaviourAngle(2,(int)direction);
								_robotMan.getRobotStatus().setBehaviourMagnitude(2,(int)magnitude);
							}
							else if( b instanceof Travel)
							{
								_robotMan.getRobotStatus().setBehaviourAngle(3,(int)direction);
								_robotMan.getRobotStatus().setBehaviourMagnitude(3,(int)magnitude);	
							}
							else if( b instanceof RandomDrive)
							{
								_robotMan.getRobotStatus().setBehaviourAngle(4,(int)direction);
								_robotMan.getRobotStatus().setBehaviourMagnitude(4,(int)magnitude);	
							}
							
							
							/*
							// Update behavior status with new values
							if (behaviourNum < RobotStatusMsg.MaxBehaviorsInStatus)
							{
								// Direction and magnitude
								_robotMan.getRobotStatus().setBehaviourAngle(behaviourNum,(int)direction);
								_robotMan.getRobotStatus().setBehaviourMagnitude(behaviourNum,(int)magnitude);
							}
							behaviourNum++;
							*/
							
							//_pilot.setMoveSpeed((int)(max_speed*magnitude)/10);
							
							// Lookup behavior priority
							priority = (Integer)_behaviors.get(key);
							magnitude = magnitude * priority;
							
							// Convert to x, y
							direction = (direction*(Math.PI/180));
							x1 = Math.cos(direction)*magnitude;
							y1 = Math.sin(direction)*magnitude;

							// Add to total x, y
							x = x + x1;
							y = y + y1;						
						}
					}
					
					// Calculate total magnitude and angle
					magnitude = Math.sqrt(x*x + y*y);
					int angle = (int)(Math.atan(y/x)*(180/Math.PI));
					
					// Correct angle if required
					if (x<0 && y>0) {angle+=180;}
					if (x<0 && y<=0) {angle+=180;}
					if (x>0 && y<0) {angle+=360;}
					
					// Extract heading from pilot and ensure between 0 and 359
					int heading = Math.round(getAngle());
				    while(heading>=360)heading -= 360;
				    while(heading<0)heading += 360;
				    LCD.drawString("Heading: "+heading+"   ", 0, 2);
					
					// Update robot status with new values
				    _robotMan.getRobotStatus().setHeading(heading);
					_robotMan.getRobotStatus().setAngle(angle);
					//_robotMan.getRobotStatus().setMagnitude((int)magnitude);
					
					LCD.drawString(banan+ "", 0, 3);
					banan++;
					LCD.drawString(2+"   ", 10, 3);
					
					if(magnitude < 0.001)
					{
						LCD.drawString(3+"   ", 10, 3);
						stop();
					}
					else {
						//LCD.drawString("Angle: " + angle, 0, 0);
						LCD.drawString(4+"   ", 10, 3);
						
						// Calculate error
						int error = heading - angle;
						// Handle the wrap-around problem
						while (error < -180) error = error + 360;
						while (error > 180) error = error - 360;

			            //LCD.drawString("error: " + error + "    ", 0, 5);
			            

						LCD.drawString(5+"   ", 10, 3);
			            
			            if( _useSteer )
			            {
							LCD.drawString(6+"   ", 10, 3);
							// Add gain to error. TODO: Adjust gain if needed
				            float gain = -1;
				            error = (int)(gain * error);
				            
				            // Use pilot steer to adjust course
				            steer(FORWARD * error, 360 * FORWARD,true);
				            
				            // Continue code and adjust soon again
			            }
			            else
			            {
							
			            	int abs_error = Math.abs(error);
			            	LCD.drawString("" + error + "  ", 10, 3);
			            	_robotMan.getRobotStatus().setMagnitude((int)error);
			            	
			            	if( abs_error > 10)
				            {
			            		if( abs_error > 160 )
			            		{
			            			
			            			backward();
			            			
			            			Common.sleep(1000);
			            		}
			            		
			            		
			            		if( _pilot instanceof CompassPilot)
			            		{
			            			rotateTo(angle, false);
			            			stop();
			            		}
			            		else
			            		{
			            			// Tacho
			            			rotate(error*-1, false);
			            			stop();
			            		}
			            		
				            	forward();
								
				        
				            	Common.sleep(100); // Give time to drive some
				            }
			            }
			            
					}
					Common.sleep(10);
				}
			}
		};
	}
	
	public void start()
	{
		_thread.setDaemon(true);
		_thread.start();
		_started = true;
	}

	public void addBehavior(Behavior behavior, int priority)
	{
		if( !_started )
		{
			_behaviors.put(behavior, priority);
		}
	}

	public float getAngle()
	{
		float angle;
		synchronized (_pilot) 
		{
			angle = _pilot.getAngle();
		}
		return angle;
	}
	
	private void forward()
	{
		synchronized (_pilot) {
			_pilot.forward();
		}
	}
	
	private void backward()
	{
		synchronized (_pilot) {
			_pilot.backward();
		}
	}
	
	private void stop()
	{
		synchronized (_pilot) {
			_pilot.stop();
		}
	}
	
	private void rotateTo(float angle, boolean immediateReturn)
	{
		synchronized (_pilot) {
			((CompassPilot)_pilot).rotateTo(angle,immediateReturn);
		}
	}
	
	private void rotate(float angle, boolean immediateReturn)
	{
		synchronized (_pilot) {
			_pilot.rotate(angle,immediateReturn);
		}
	}
	
	private void steer(float turnRate, float angle, boolean immediateReturn)
	{
		synchronized (_pilot) {
			_pilot.steer(turnRate, angle, immediateReturn);
		}
	}
}
